/*
 * autogen_ekf_propagation.c
 *
 * Embedded MATLAB Coder code generation for function 'autogen_ekf_propagation'
 *
 * C source code generated on: Wed Aug 24 18:34:48 2011
 *
 */

/* Include files */
#include "rt_nonfinite.h"
#include "autogen_ekf_propagation.h"

/* Type Definitions */

/* Named Constants */

/* Variable Declarations */

/* Variable Definitions */

/* Function Declarations */

/* Function Definitions */
void autogen_ekf_propagation(const real32_T eml_in1[20], const real32_T eml_in2
  [3], const real32_T eml_in3[3], real32_T eml_dt,
  real32_T eml_x_new[16])
{
  real32_T eml_t17;
  real32_T eml_t18;
  real32_T eml_t19;
  real32_T eml_t20;
  real32_T eml_t21;
  real32_T eml_t22;
  real32_T eml_t23;
  real32_T eml_t24;
  real32_T eml_t25;
  real32_T eml_t26;
  real32_T eml_t27;
  real32_T eml_t28;
  real32_T eml_t29;
  real32_T eml_t30;

  /* AUTOGEN_EKF_PROPAGATION */
  /*     X_NEW = AUTOGEN_EKF_PROPAGATION(IN1,IN2,IN3,DT) */
  /*     This function was generated by the Symbolic Math Toolbox version 5.5. */
  /*     24-Aug-2011 18:34:47 */
  eml_t17 = eml_in2[1] - eml_in1[14];
  eml_t18 = rt_pow32_snf(eml_in1[6], 2.0F);
  eml_t19 = rt_pow32_snf(eml_in1[7], 2.0F);
  eml_t20 = rt_pow32_snf(eml_in1[8], 2.0F);
  eml_t21 = rt_pow32_snf(eml_in1[9], 2.0F);
  eml_t22 = eml_in2[0] - eml_in1[13];
  eml_t23 = eml_in1[7] * eml_in1[8] * 2.0F;
  eml_t24 = eml_in2[2] - eml_in1[15];
  eml_t25 = eml_in1[6] * eml_in1[8] * 2.0F;
  eml_t26 = eml_in1[7] * eml_in1[9] * 2.0F;
  eml_t27 = eml_in1[6] * eml_in1[7] * 2.0F;
  eml_t28 = eml_in3[0] - eml_in1[10];
  eml_t29 = eml_in3[1] - eml_in1[11];
  eml_t30 = eml_in3[2] - eml_in1[12];
  eml_x_new[0] = eml_in1[0] + eml_dt * eml_in1[3];
  eml_x_new[1] = eml_in1[1] + eml_dt * eml_in1[4];
  eml_x_new[2] = eml_in1[2] + eml_dt * eml_in1[5];
  eml_x_new[3] = eml_in1[3] + eml_dt * ((eml_t24 * (eml_t25 + eml_t26) + eml_t22
    * (((eml_t18 + eml_t19) - eml_t20) - eml_t21)) +
    eml_t17 * (eml_t23 - eml_in1[6] * eml_in1[9] * 2.0F));
  eml_x_new[4] = eml_in1[4] + eml_dt * ((eml_t17 * (((eml_t18 - eml_t19) +
    eml_t20) - eml_t21) + eml_t22 * (eml_t23 + eml_in1[6]
    * eml_in1[9] * 2.0F)) - eml_t24 * (eml_t27 - eml_in1[8] * eml_in1[9] * 2.0F));
  eml_x_new[5] = eml_in1[5] - eml_dt * (((eml_t22 * (eml_t25 - eml_t26) -
    eml_t24 * (((eml_t18 - eml_t19) - eml_t20) + eml_t21))
    - eml_t17 * (eml_t27 + eml_in1[8] * eml_in1[9] * 2.0F)) + 9.81F);
  eml_x_new[6] = ((eml_in1[6] - eml_dt * eml_t28 * eml_in1[7] * 0.5F) - eml_dt *
                  eml_t29 * eml_in1[8] * 0.5F) - eml_dt * eml_t30
    * eml_in1[9] * 0.5F;
  eml_x_new[7] = ((eml_in1[7] + eml_dt * eml_t28 * eml_in1[6] * 0.5F) - eml_dt *
                  eml_t29 * eml_in1[9] * 0.5F) + eml_dt * eml_t30
    * eml_in1[8] * 0.5F;
  eml_x_new[8] = ((eml_in1[8] + eml_dt * eml_t28 * eml_in1[9] * 0.5F) + eml_dt *
                  eml_t29 * eml_in1[6] * 0.5F) - eml_dt * eml_t30
    * eml_in1[7] * 0.5F;
  eml_x_new[9] = ((eml_in1[9] - eml_dt * eml_t28 * eml_in1[8] * 0.5F) + eml_dt *
                  eml_t29 * eml_in1[7] * 0.5F) + eml_dt * eml_t30
    * eml_in1[6] * 0.5F;
  eml_x_new[10] = eml_in1[10];
  eml_x_new[11] = eml_in1[11];
  eml_x_new[12] = eml_in1[12];
  eml_x_new[13] = eml_in1[13];
  eml_x_new[14] = eml_in1[14];
  eml_x_new[15] = eml_in1[15];
}

/* End of Embedded MATLAB Coder code generation (autogen_ekf_propagation.c) */
